function [x1,f1,B1] = irismin(This,x0,varargin)
% irismin  (BETA) Quasi-Newton line search and trust region gradient-based function minimization.
%
% Syntax
% =======
%
%     [X1,F,Hess] =  irisoptim.irismin(Fun,X0,...)
%
% Input arguments
% ================
%
% * `Fun` [ function_handle ] - Handle to the function to be optimized.
%
% * `X0` [ numeric ] - Initial value.
%
% Output arguments
% =================
%
% * `X1` [ numeric ] - Optimized vector.
%
% * `F` [ numeric ] - Final objective function value.
%
% * `Hess` [ numeric ] - Final approximate Hessian.
%
% Options
% ========
%
% * `'diffMethod='` [ *'forward'* | 'central' ] - Finite differencing
% method for calculating gradient. 
% 
% * `'display='` [ `'off'` | `'final'` | *`'iter'`* ] - Level of display in
% order of increasing verbosity.
%
% * `'maxFunEvals='` [ numeric | *1e+5* ] - Maximum number of function
% evaluations.
%
% * `'maxIter='` [ numeric | *1e+4* ] - Maximum number of iterations.
%
% * `'step='` [ *'trust-region'* | 'linesearch' ] - Step method.
%
% * `'tolfun='` [ numeric | *1e-6* ] - Minimum change in objective function
% value.
% 
% * `'updateInterval='`* [ numeric | `1` ] - Minimum length of time in
% seconds which must pass before new command window output will be
% produced.
%
% * `'update='` [ 'damped-bfgs' | *'bfgs'* | 'dfp' | 'psb' ] - 
% Approximate Hessian updating algorithm. BFGS selects
% Broyden-Fletcher-Goldfarb-Shanno, DFP selects Davidon-Fletcher-Powell,
% PSB selects Powell-symmetric-Broyden. 
%
% * `'wolfeParams='` [ numeric | *[1e-4,.9]* ] - Parameters relating
% to backtracking and the Wolfe conditions.
%
% Description
% ============
%
%
% Examples
% =========
%
%
% References
% ===========
%
% * Denis, J.E.; and Schnabel, Robert B. (1983) "Numerical Methods for
% Unconstrained Optimization and Nonlinear Equations," Siam. 
% 
% * Conn, A.R.; Gould, N.I.M.; and Toint, Ph.L. (1991). "Convergence of
% quasi-Newton matrices generated by the symmetric rank one update,"
% Mathematical Programming.

% -IRIS Toolbox.
% -Copyright (c1) 2007-2015 IRIS Solutions Team.

options = passvalopt('irisoptim.irismin',varargin{:});
e = 2*sqrt(eps) ;
ec = eps^(1/3) ;
x0 = x0(:) ;
x1 = x0 ;

nx = numel(x0) ;
xch = Inf ;
fch = Inf ;

if strcmpi(options.stepMethod,'linesearch') && strcmpi(options.updateMethod,'sr1')
    utils.error('irisoptim:irismin',...
        ['Symmetric rank 1 updates should not be used with line search ',...
        'methods because they do not ensure the approximate Hessian is ',...
        'positive definite.']) ;
end
if strcmpi(options.stepMethod,'trust-region') && strcmpi(options.trustregionMethod,'dogleg') && strcmpi(options.updateMethod,'sr1')
        utils.error('irisoptim:irismin',...
        ['Symmetric rank 1 updates should not be used with trust region ',...
        'dogleg because the dogleg requires a positive definite approximate Hessian.']) ;
end
if options.tolfun>=options.wolfeParams(1)
    utils.error('irisoptim:irismin','Invalid Wolfe parameters.') ;
end

nFunEvals = 0 ;
[g0,f0] = xxGrad(This,x0) ;
f1 = f0 ; B1 = [] ;
if strcmpi(options.display,'iter')
    fprintf(1,['Minimizing a function of %g variables using step method %s and ',...
        'approximate Hessian update method %s.\n'],nx,options.stepMethod,options.updateMethod) ;
    fprintf(1,'[%g] Obj = %g\n',0,f0) ;
end
if strcmpi(options.hessian,'identity')
    B0 = eye(nx) ;
else
    B0 = options.hessian ;
    assert(size(B0,1)==nx) ;
    assert(size(B0,2)==nx) ;
end
it = 0 ;
switch options.stepMethod
    case 'trust-region'
        % t is the trust region radius
        t = 1 ;
        isLineSearch = false ;
    otherwise
        % t is the backtracking value
        isLineSearch = true ;
        lineSearchSuccess = false ;
        c1 = options.wolfeParams(1) ;
        c2 = options.wolfeParams(2) ;
end
time = tic ;
crit = norm(g0)/max(norm(x0),1) ;
while crit>e && xch>options.tolx ...
        && it<=options.maxit ...
        && nFunEvals<options.maxfunevals ...
        && norm(g0)>options.tolfun
    it = it+1 ;
    
    if isLineSearch
        direction = -B0\g0 ;
        [step,lineSearchSuccess,f1,g1,t] = lineSearch(x0,direction,f0,g0) ;
    else
        % trust-region
        step = trStep(g0,B0,t) ;
        [g1,f1] = xxGrad(This,x0+step) ;
    end
    
    aRed = f0 - f1 ;
    switch options.stepMethod
        case 'linesearch'
            x1 = x0 + step ;
            xch = max(abs(step)) ;
        case 'trust-region'
            pRed = - ( g0'*step + 0.5*step'*B0*step ) ;
            [t,x1,xch] = doTrUpdate(t,x0,step,aRed,pRed) ;
    end
    
    B1 = doHessUpdate(B0,g0,g1,step) ;
    
    crit = norm(g1)/max(norm(x1),1) ;
    pchobj = (f0-f1)/f1 ;
    if toc(time)>options.updateInterval || it==1
        if strcmpi(options.display,'iter')
            fprintf(1,'[%g] Obj = %g, pctch(f) = %g, rnorm(g1) = %g, dx = %g, t = %g, nf = %g\n',it,f1,pchobj,crit,xch,t,nFunEvals) ;
        end
        time = tic ;
    end
    x0 = x1 ;
    f0 = f1 ;
    g0 = g1 ;
    if isLineSearch && ~lineSearchSuccess
        break ;
    end
end
doDispLast(it,f0,x0,xch,fch) ;

    function step = trStep(g0,B0,t)
        switch options.trustregionMethod
            case 'dogleg'
                sd = - ((g0'*g0)/(g0'*B0*g0))*g0 ;
                if norm(sd) >= t
                    % steepest descent path leads outside trust region
                    step = t*(sd/norm(sd)) ;
                else
                    fs = -B0\g0 ;
                    if norm(fs) <= t
                        step = fs ;
                    else
                        mfh = @(x) norm(sd + (x-1)*(fs-sd))-t ;
                        xStar = irisoptim.brent(mfh,1,2,'display=','off') ;
                        step = sd + (xStar-1)*(fs-sd) ;
                    end
                end
        end
    end

    function [step,success,f1,g1,t] = lineSearch(x0,direction,f0,g0) 
        % finds the longest step length which satisfies the Wolfe
        % conditions
        switch options.linesearchMethod
            case 'interp'
                [t,success,f1,g1] = doGoalAttain(x0,direction,f0,g0)  ;
            case {'backtracking','backtrack'}
                [t,success,f1,g1] = doBacktrack(x0,direction,f0,g0)  ;
            case 'none'
                [f1,g1] = xxGrad(This,x0+direction) ;
                if f1<f0
                    success = true ;
                    t = 1 ;
                else
                    success = false ;
                    t = 0 ;
                    f1 = f0 ;
                    g1 = g0 ;
                end
        end
        t = max(t,sqrt(eps)) ;
        step = t*direction ;
    end

    function a2 = doCubic(a0,a1,p0,pa0,pa1,Dp0)
        ab = ( 1/(a0^2*a1^2*(a1-a0)) )...
            *[a0^2,-a1^2;-a0^3,a1^3]...
            *[pa1-p0-a1*Dp0; pa0-p0-a0*Dp0] ;
        a2 = (-ab(2)+sqrt(ab(2)^2-3*ab(1)*Dp0)) / (3*ab(1)) ;
    end

    function [t,success,f1,g1,it]=doGoalAttain(x0,direction,f0,g0) 
        success = false ;
        % try full quasi newton step first
        t0 = 1 ;
        f1 = This(x0+t0*direction) ;
        nFunEvals = nFunEvals + 1 ;
        if armijoCondition(t0,direction,f0,f1,g0)
            t = t0 ;
            success = true ;
            [g1,f1] = xxGrad(This,x0+t*direction) ;
            return ;
        else
            fBest = f1 ;
            aBest = t0 ;
            % full newton step doesn't satisfy armijo, pick next
            % quadratically
            p0 = f0 ;
            Dp0 = g0'*direction ;
            pa0 = f1 ;
            t1 = -Dp0 / (2*(pa0-p0-Dp0)) ;
            f2 = This(x0+t1*direction) ;
            if f2<fBest
                fBest = f2 ;
                aBest = t1 ;
            end
            nFunEvals = nFunEvals + 1 ;
            if armijoCondition(t1,direction,f0,f2,g0)
                t = t1 ;
                [g1,f1] = xxGrad(This,x0+t1*direction) ;
                success = true ;
                return ;
            else
                it = 0 ;
                % recursive cubic
                pa1 = f2 ;
                maxit = 1e+2 ;
                while it<maxit
                    it = it + 1 ;
                    t2 = doCubic(t0,t1,p0,pa0,pa1,Dp0) ;
                    f2 = This(x0+t2*direction) ;
                    if f2<fBest
                        fBest = f2 ;
                        aBest = t2 ;
                    end
                    nFunEvals = nFunEvals + 1 ;
                    if armijoCondition(t2,direction,f0,f2,g0)
                        success = true ;
                        t = t2 ;
                        [g1,f1] = xxGrad(This,x0+t*direction) ;
                        return ;
                    else
                        t0 = t1 ;
                        t1 = t2 ;
                        pa0 = pa1 ;
                        pa1 = f2 ;
                    end
                end
                t = aBest ;
                [g1,f1] = xxGrad(This,x0+t*direction) ;
            end
        end
    end

    function X = wolfeConditions(t,direction,f0,f1,g0,g1)
        % reduction should be propotional to step length and gradient
        X1 = armijoCondition(t,direction,f0,f1,g0) ;
        % curvature condition (strong version)
        X2 = abs(direction'*g1) <= c2*abs(direction'*g0) ;
        X = X1 && X2 ;
    end

    function X = armijoCondition(t,direction,f0,f1,g0)
        % armijo condition: reduction should be proportional to step length
        % and gradient
        X = f1 <= f0 + c1*t*direction'*g0 ;
    end

    function [g,lf0] = xxGrad(fun,x)
        lf0 = This(x) ;
        g = NaN(nx,1) ;
        
        switch options.diffMethod
            case 'forward'
                for ii = 1:nx
                    ee = e*max(abs(x(ii)),1)*sign(x(ii)) ;
                    tmp = x(ii) + ee ;
                    ee = tmp - x(ii) ;
                    xf = x ;
                    xf(ii) = x(ii) + ee ;
                    g(ii) = (fun(xf)-lf0)/ee ;
                end
                nFunEvals = nFunEvals + nx + 1 ;
            case 'central'
                for ii = 1:nx
                    x1 = x ;
                    xf = x ;
                    xb = x ;
                    ee = e*max(abs(x(ii)),1)*sign(x(ii)) ;
                    tmp = x(ii) + ee ;
                    ee = tmp - x(ii) ;
                    xf(ii) = x(ii) + ee/2 ;
                    xb(ii) = x(ii) - ee/2 ;
                    g(ii) = (fun(xf)-fun(xb))/ee ;
                end
                nFunEvals = nFunEvals + 2*nx + 1 ;
        end
    end

    function B1 = doHessUpdate(B0,g0,g1,step)
        dg = g1 - g0 ;
        if strcmpi(options.updateMethod,'damped-bfgs')
            sBs = step'*B0*step ;
            if step'*dg >= 0.2*sBs
                tk = 1 ;
            else
                tk = ( 0.8*sBs ) / ( sBs - step'*dg ) ;
            end
            r = tk*dg + (1-tk)*B0*step ;
            B1 = B0 - ( B0*(step*step')*B0 )/sBs + ( r*r' )/( step'*r ) ;
        else
            r = dg - B0*step ;
            switch options.updateMethod
                case 'bfgs'
                    B1 = B0 - ( B0*(step*step')*B0 )/(step'*B0*step) ...
                        + (dg*dg')/(dg'*step) ;
                case 'sr1'
                    if abs(r'*step)>=1e-8*norm(r)*norm(step)
                        B1 = B0 + ( r*r' )/( r'*step ) ;
                    else
                        % no hessian update
                    end
                case 'dfp'
                    B1 = B0 + ( r*dg'+dg*r' )/(dg'*step) ...
                        - ( (r'*step)*(dg*dg') )/(dg'*step)^2 ;
                case 'psb'
                    B1 = B0 + ( r*step' + step*r' )/( step'*step ) ...
                        - ( (r'*step)*(step*step') )/( step'*step )^2 ;
            end
        end
    end

    function [t,x1,xch] = doTrUpdate(t,x0,step,aRed,pRed)
        rat = aRed/pRed ;
        
        % take step
        if rat > 1e-3
            x1 = x0 + step ;
            xch = max(abs(step)) ;
        else
            x1 = x0 ;
            xch = Inf ;
        end
        
        if rat > 0.75
            if norm(step)>0.8*t
                t = t*1.2 ;
            end
        elseif rat<0.1
            t = max(t/1.2,sqrt(eps)) ;
        end
    end

    function [t,success,f1,g1,it] = doBacktrack(x0,direction,f0,g0) 
        t = 1 ;
        it = 0 ;
        maxit = 1e+2 ;
        if options.strong
            [g1,f1] = xxGrad(This,x0+direction) ;
            fBest = f1 ;
            aBest = 1 ;
            gBest = g1 ;
            while ~wolfeConditions(t,direction,f0,f1,g0,g1) && it<maxit
                it = it + 1 ;
                t = c2*t ;
                [g1,f1] = xxGrad(This,x0+t*direction) ;
                if f1<fBest
                    fBest = f1 ;
                    aBest = t ;
                    gBest = g1 ;
                end
            end
            if fBest<f1
                f1 = fBest ;
                g1 = gBest ;
                t = aBest ;
            end
        else
            f1 = This(x0+direction) ;
            fBest = f1 ;
            aBest = 1 ;
            nFunEvals = nFunEvals + 1 ;
            while ~armijoCondition(t,direction,f0,f1,g0) && it<maxit
                it = it + 1 ;
                t = c2*t ;
                f1 = This(x0+t*direction) ;
                nFunEvals = nFunEvals + 1 ;
                if f1<fBest
                    fBest = f1 ;
                    aBest = t ;
                end
            end
            if fBest<f1
                t = aBest ;
            end
            [g1,f1] = xxGrad(This,x0+t*direction) ;
        end
        if it==maxit
            success = false ;
        else
            success = true ;
        end
    end

    function doDispLast(it,f1,x1,xch,fch)
        if any(strcmpi(options.display,{'iter','final'}))
            fprintf(1,'Final obj = %g\n',f1) ;
            fprintf(1,'Terminating at %s.\n',mat2str(x1)) ;
            if isLineSearch && ~lineSearchSuccess
                fprintf(1,'Backtracking failed to find a step length which satisfies the Wolfe conditions.\n') ;
            end
            if fch<=options.tolfun
                fprintf(1,'Objective gradient norm less than specified tolerance.\n') ;
            end
            if xch<=options.tolx
                fprintf(1,'Change in x less than specified tolerance.\n') ;
            end
            if it>=options.maxit
                fprintf(1,'Maximum number of iterations reached.\n') ;
            end
        end
    end

end
